#pragma once
#include <Eigen/Core>

#ifndef savgol_print_matrix
#define savgol_print_matrix(x)                                                                     \
    do                                                                                             \
    {                                                                                              \
        Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");        \
        std::cout << #x << " " << __func__ << ":" << __LINE__ << " " << x.rows() << ","            \
                  << x.cols() << std::endl;                                                        \
        std::cout << x.format(HeavyFmt) << std::endl;                                              \
    } while (false)
#endif

namespace numpy {
/// @brief Least squares polynomial fit.
/// .. note::
///    This forms part of the old polynomial API. Since version 1.4, the
///    new polynomial API defined in `numpy.polynomial` is preferred.
///    A summary of the differences can be found in the
///    :doc:`transition guide </reference/routines.polynomials>`.
///
/// Fit a polynomial ``p(x) = p[0] * x**deg + ... + p[deg]`` of degree `deg`
/// to points `(x, y)`. Returns a vector of coefficients `p` that minimises
/// the squared error in the order `deg`, `deg-1`, ... `0`.
///
/// The `Polynomial.fit <numpy.polynomial.polynomial.Polynomial.fit>` class
/// method is recommended for new code as it is more stable numerically. See
/// the documentation of the method for more information.
/// @param x array_like, shape (M,)
/// x-coordinates of the M sample points ``(x[i], y[i])``.
/// @param y array_like, shape (M,) or (M, K)
/// y-coordinates of the sample points. Several data sets of sample
/// points sharing the same x-coordinates can be fitted at once by
/// passing in a 2D-array that contains one dataset per column.
/// @param deg int
/// Degree of the fitting polynomial
/// @return ndarray, shape (deg + 1,)
///     Polynomial coefficients, highest power first.  If `y` was 2-D, the
///     coefficients for `k`-th data set are in ``p[:,k]``.
///
inline Eigen::VectorXd polyfit(Eigen::RowVectorXd x, Eigen::RowVectorXd y, int deg)
{
    if (deg < 0)
    {
        return {};
    }
    int order = deg + 1;
    // lhs = vander(x, order)
    Eigen::MatrixXd lhs = Eigen::MatrixXd::Zero(x.cols(), order);
    for (int r = 0; r < order; r++)
    {
        lhs.col(order - 1 - r) = x.array().pow(r);
    }
    Eigen::VectorXd rhs = y;
    // Find the least-squares solution of lhs*c = rhs
    Eigen::VectorXd coeffs = lhs.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(rhs);
    return coeffs;
}
inline Eigen::VectorXd polyder(Eigen::VectorXd p)
{
    if (p.size() <= 1)
    {
        return Eigen::VectorXd::Zero(1);
    }
    Eigen::VectorXd res = Eigen::VectorXd::Zero(p.size() - 1);
    for (int i = 0; i < res.size(); ++i)
    {
        res(i) = p(i) * (res.size() - i);
    }
    return res;
}
inline Eigen::VectorXd polyder(Eigen::VectorXd p, int m)
{
    if (m <= 0)
    {
        return p;
    }
    Eigen::VectorXd res = p;
    for (int k = 0; k < m; k++)
    {
        res = polyder(res);
    }
    return res;
}
inline Eigen::VectorXd polyval(Eigen::VectorXd p, Eigen::VectorXd x)
{
    Eigen::VectorXd y = Eigen::VectorXd::Zero(x.size());
    for (int r = 0; r < x.rows(); r++)
    {
        double res = 0;
        std::for_each(p.cbegin(), p.cend(), [&](auto c) { res = std::fmal(res, x(r), c); });
        y(r) = res;
    }
    return y;
}
} // namespace numpy
